超音波レンジファインダー

構想２年，製作1日．
レーザレンジファインダーが高価で買えないなぁ．．．と思って超音波で似たようなものを作れないかと思っていたところ，資金繰りができてレーザレンジが買えてしまったので作らずじまいだったのですが，最近お手軽な移動ロボットをあれこれ考えているところであったら便利かなと思って今日作ってみました．

超音波センサとサーボを組合わせてシリアル通信で制御してみました↓

回路はこんな感じ↓　サーボの電源に１０Ωの抵抗を入れているところが工夫したところ．
PWM制御のサーボでは指定角度になっているかどうかが分かりませんが，ここで電圧降下をモニタすると指定角度に落ち着いているかどうか判断できます．

// 制御コマンド
// a: 全データ読み出し
// サーボスタート位置+CR+LF，サーボエンド位置+CR+LF，
// １０度毎のデータ（cm）をコンマ区切りでデータの個数だけ送信，最後は+CR+LF
// r＋数値: 指定角度データ読み出し（角度固定時はｒのみで固定角度の距離読み出し）
// f: 角度固定
// g: 指定範囲でスキャン
// s＋数値: スキャンスタート位置を設定
// e＋数値: スキャンエンド位置を設定
// d: サーボのスキャン範囲をデフォルト値に戻す（０から１８０）

動画はこちら→超音波レンジファインダー

スケッチダウンロード

// -----------------------------------------------------------------
// 超音波レンジファインダー
//
// 超音波センサとサーボによる１次元距離マップの作成
// シリアル通信による設定とデータの読み出し
// シリアル通信によるステアリングとアンプの制御
//
// 制御コマンド
// a: 全データ読み出し
// サーボスタート位置+CR+LF，サーボエンド位置+CR+LF，
// １０度毎のデータ（cm）をコンマ区切りでデータの個数だけ送信，最後は+CR+LF
// r＋数値: 指定角度データ読み出し（角度固定時はｒのみで固定角度の距離読み出し）
// f: 角度固定
// g: 指定範囲でスキャン
// s＋数値: スキャンスタート位置を設定
// e＋数値: スキャンエンド位置を設定
// d: サーボのスキャン範囲をデフォルト値に戻す（０から１８０）
//
// 2011.4.3: Ver.1
// -----------------------------------------------------------------

#include < Servo.h > // ←修正してください

#define SRV 9 // サーボの信号
#define SRVMIN 250 // サーボの電圧モニタ
#define SRVMAX 2200 // サーボの電圧モニタ
#define SRVS 0 // サーボのスタート位置のデフォルト
#define SRVE 180 // サーボのエンド位置のデフォルト
#define PW 2 // 超音波センサパルス幅
#define SRVV 0 // サーボの電圧モニタ

int pos=90; // サーボ位置
int posS=SRVS; // サーボのスタート位置設定
int posE=SRVE; // サーボのエンド位置設定
int dir=0; // スキャン方向
int urf[19]; // 超音波センサの読み取り値をストアする配列，０から１８０度，１０度きざみ
int fscan=1; // センサをスキャンするかどうかのフラグ，　＝１：スキャンする，＝０：固定
Servo scanServo;

// ---------------- 関数定義 ----------------------------------------

// 文字列読み取り関数，バッファ読み出しが終わったら区切る，1文字入力も可
void recvStr2(char *buf)
{
int i = 0;
char c;
while (Serial.available()) {
c = Serial.read();
buf[i] = c;
i++;
}
buf[i] = '\0'; // 文字列終わり
}

// Commandコントロール
void command()
{
char str[10], str2[10];
//int dat;

// キー入力の確認，モータ回転
if (Serial.available()){
recvStr2(str); // 文字列読み取り
//Serial.print(str); // エコーバック
switch (str[0]){ // １文字でも文字列として読取るので，最初の１字を抜き出して分岐
case 'a': readall(); break; // 全データ読み出し
case 'r': strcpy(str2, str+1);
if (fscan==1) readdat(atoi(str2)); else readdat(pos);
break;
case 'f': fscan=0; strcpy(str2, str+1); pos=atoi(str2); break; // 角度固定
case 'g': fscan=1; break; // スキャンさせる
case 's': strcpy(str2, str+1); posS=((int)(atoi(str2)+5)/10)*10; break; // スキャンスタート位置を設定
case 'e': strcpy(str2, str+1); posE=((int)(atoi(str2)+5)/10)*10; break; // スキャンエンド位置を設定
case 'd': fscan=1; posS=SRVS; posE=SRVE; break; // サーボのスキャン範囲をデフォルト値に戻す
}
}
}

// 全データを読み出す
void readall(){
int i;
Serial.println(posS);
Serial.println(posE);
for (i=posS/10; i <= (posE/10); i++){
Serial.print(urf[i]);
if (i==(posE/10)) Serial.println(); else Serial.print(",");
}
}

// 指定角度のデータを読み出す
void readdat(int dat){
Serial.println( urf[(dat+5)/10] ); // 10度以下は四捨五入
}

// ---------------- セットアップ ----------------------------------------
void setup()
{
pinMode(PW, INPUT); // 超音波センサパルス幅

scanServo.attach(SRV,SRVMIN,SRVMAX);
Serial.begin(115200); // BT接続に使用

}

// ---------------- メインループ ----------------------------------------
void loop()
{

// サーボの制御
if (fscan==1){
if (dir==1){
pos+=10; if (pos > posE) {pos=posE; dir=0;}
} else {
pos-=10; if (pos < posS) {pos=posS; dir=1; }
}
}
scanServo.write(pos); // サーボに指示
delay(50); // サーボ回し始めをちょっと待つ
while(analogRead(SRVV) < 1023) delay(10); // アナログ値を連続して読取るときディレイを入れないとミスが起きる
urf[pos/10]=pulseIn(PW,HIGH,200000)/58; // 147us/inch

// コマンド操作
command();
}

広告↓